/*
  IPG-300W Laser
*/


#ifndef ULAPI
#error This is intended as a userspace component only.
#endif

#ifdef DEBUG
#define DBG(fmt, ...)                   \
    do {                        \
    if (param.debug) printf(fmt,  ## __VA_ARGS__);  \
    } while(0)
#else
#define DBG(fmt, ...)
#endif

#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <ctype.h>
#include <stdlib.h>
#include <signal.h>
#include <errno.h>
#include <getopt.h>
#include <math.h>
#include <signal.h>
#include <stdarg.h>
#include <stdbool.h>

#include "rtapi.h"
#include "hal.h"
#include <modbus.h>
#include <modbus-tcp.h>
#include "inifile.h"

#include <termios.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/select.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include<arpa/inet.h>

#include <netinet/ip.h>
#include <netinet/tcp.h>
#include <netdb.h>



typedef enum
{
    pulse_model = 7,
    continue_model = 8,
} lasermodel_e;



/* HAL data struct */
typedef struct
{
    //Laser output
    hal_u32_t   *status;
    hal_float_t *circuit_borad_temperature;
    hal_float_t *device_temperature;
    hal_float_t *output_power; // w
    hal_float_t *current_setpoint; // %
    hal_float_t *min_current_setpoint; // min value to set current setpoint %
    hal_float_t *current_monitor; // A
    hal_float_t *output_peak_power; // w
    hal_float_t *pulse_repetition_rate; // Hz
    hal_float_t *pulse_width; // ms
    hal_u32_t   *error_code;
    hal_u32_t   *apd_mode_index; // 7:pulse    8:continue
    hal_bit_t   *modbus_ok; // the last MODBUS_OK transactions returned successfully

    //Laser input
    hal_float_t *prr_cmd; //Hz
    hal_float_t *pulse_width_cmd; //ms
    hal_float_t *diode_current_cmd; // %  0-100
    hal_float_t  *apd_mode_index_cmd; // 7:pulse    8:continue
    hal_bit_t   *emission_cmd;  // 0 off   1 on
    hal_bit_t   *guide_cmd; // 0 off   1 on
    hal_bit_t   *enabled;

    //
    hal_float_t looptime;
    hal_float_t speed_tolerance;

} haldata_t;



// configuration and execution state
typedef struct params
{
    int type;
    char *modname;
    struct timeval response_timeout;
    struct timeval byte_timeout;
    char *progname;
    char *section;
    FILE *fp;
    char *inifile;
    int reconnect_delay;
    int fd;
    haldata_t *haldata;
    int hal_comp_id;
    int read_initial_done;
    int modbus_ok;
    int report_device;
    int tcp_portno;
    char *tcp_destip;
    int sockfd;
    struct sockaddr_in servaddr;

    float old_prr;
    float old_pulse_width_cmd;
    float old_diode_current_cmd; // %  0-100
    float old_apd_mode_index;//16bit integer
    volatile bool      old_emission;
    volatile bool      old_guide;
} params_type, *param_pointer;

#define TYPE_RTU 0
#define TYPE_TCP_SERVER 1
#define TYPE_TCP_CLIENT 2
#define TYPE_CUSTOM     3

#define TCP_MAX_DATA_LENGTH 50


//#define POLLCYCLES  10
#define MODBUS_MIN_OK   10

//struct termios old_tios;

// default options; read from inifile or command line
static params_type param =
{
    .type = -1,
    .modname = NULL,
    .response_timeout = { .tv_sec = 0, .tv_usec = 500000 },
    .byte_timeout = {.tv_sec = 0, .tv_usec = 500000},
    .progname = "ipg-300W",
    .section = "IPG300W",
    .fp = NULL,
    .inifile = NULL,
    .reconnect_delay = 1,
    .fd = -1,
    .haldata = NULL,
    .hal_comp_id = -1,
    .read_initial_done = 0,
    .modbus_ok = 0,    // set modbus-ok bit if last MODBUS_OK transactions went well
    .report_device = 0,
    .tcp_portno = 10001,
    .tcp_destip = NULL,
    .sockfd = -1,

    .old_prr = 0.0,
    .old_pulse_width_cmd = 0.0,
    .old_diode_current_cmd = 0.0,
    .old_apd_mode_index = 0,
    .old_emission = false,
    .old_guide = false,

};


static int connection_state;
enum connstate {NOT_CONNECTED, OPENING, CONNECTING, CONNECTED, RECOVER, DONE};

static int init_command(int fd, haldata_t *haldata, param_pointer p);
static int get_device_temperature(int fd, haldata_t *haldata, param_pointer p);
static int get_circuit_borad_temperature(int fd, haldata_t *haldata, param_pointer p);
static int get_pulse_width(int fd, haldata_t *haldata, param_pointer p);
static int get_prr(int fd, haldata_t *haldata, param_pointer p);
static int get_device_status(int fd, haldata_t *haldata, param_pointer p);
static int get_output_power(int fd, haldata_t *haldata, param_pointer p);
static int get_current_monitor(int fd, haldata_t *haldata, param_pointer p);
static int get_peak_power(int fd, haldata_t *haldata, param_pointer p);
static int get_min_current_setpoint(int fd, haldata_t *haldata, param_pointer p);
static int set_prr(int fd, haldata_t *haldata, param_pointer p);
static int set_pulse_width(int fd, haldata_t *haldata, param_pointer p);
static int set_diode_current(int fd, haldata_t *haldata, param_pointer p);
static int set_guiding_light(int fd, haldata_t *haldata, param_pointer p);
static int set_laser_emission(int fd, haldata_t *haldata, param_pointer p);
static int set_laser_model(int fd, haldata_t *haldata, param_pointer p);
static int reset_critical_error(int fd, haldata_t *haldata, param_pointer p);
static int reset_error(int fd, haldata_t *haldata, param_pointer p);
static int read_initial(int fd, haldata_t *haldata, param_pointer p);
static int read_data(int fd, haldata_t *haldata, param_pointer p);
static int write_data(int fd, haldata_t *haldata, param_pointer p);

static char *option_string = "dhrmn:S:I:";
static struct option long_options[] =
{
    {"debug", no_argument, 0, 'd'},
    {"help", no_argument, 0, 'h'},
    {"modbus-debug", no_argument, 0, 'm'},
    {"report-device", no_argument, 0, 'r'},
    {"ini", required_argument, 0, 'I'},     // default: getenv(INI_FILE_NAME)
    {"section", required_argument, 0, 'S'}, // default section = LIBMODBUS
    {"name", required_argument, 0, 'n'},    // vfs11_vfd
    {0,0,0,0}
};


void  windup(param_pointer p)
{
    // if (p->fd)
    //tcsetattr(p->fd, TCSANOW, &old_tios);
    // close(p->fd);
    if(p->sockfd)
    {
        //rc = recv(p->sockfd, devnull, TCP_MAX_DATA_LENGTH, MSG_DONTWAIT);
        shutdown(p->sockfd, SHUT_RDWR);
        close(p->sockfd);
    }

    if (p->hal_comp_id >= 0)
        hal_exit(p->hal_comp_id);

}

static void toggle_modbus_debug(int sig)
{
//    param.modbus_debug = !param.modbus_debug;
//    modbus_set_debug(param.ctx, param.modbus_debug);
}

static void toggle_debug(int sig)
{
//    param.debug = !param.debug;
}

static void quit(int sig)
{

    switch (connection_state)
    {

        case CONNECTING:
            // modbus_tcp_accept() or TCP modbus_connect()  were interrupted
            // these wont return to the main loop, so exit here
            windup(&param);
            exit(0);
            break;

        default:
            connection_state = DONE;
            break;
    }
}

enum kwdresult {NAME_NOT_FOUND, KEYWORD_INVALID, KEYWORD_FOUND};
#define MAX_KWD 10

int findkwd(param_pointer p, const char *name, int *result, const char *keyword, int value, ...)
{
    const char *word;
    va_list ap;
    const char *kwds[MAX_KWD], **s;
    int nargs = 0;

    if ((word = iniFind(p->fp, name, p->section)) == NULL)
        return NAME_NOT_FOUND;

    kwds[nargs++] = keyword;
    va_start(ap, value);

    while (keyword != NULL)
    {
        if (!strcasecmp(word, keyword))
        {
            *result = value;
            va_end(ap);
            return KEYWORD_FOUND;
        }
        keyword = va_arg(ap, const char *);
        kwds[nargs++] = keyword;
        if (keyword)
            value = va_arg(ap, int);
    }
    fprintf(stderr, "%s: %s:[%s]%s: found '%s' - not one of: ",
            p->progname, p->inifile, p->section, name, word);
    for (s = kwds; *s; s++)
        fprintf(stderr, "%s ", *s);
    fprintf(stderr, "\n");
    va_end(ap);
    return KEYWORD_INVALID;
}

int read_ini(param_pointer p)
{
    const char *s;

    if ((p->fp = fopen(p->inifile,"r")) != NULL)
    {

        iniFindInt(p->fp, "PORT", p->section, &p->tcp_portno);
        if ((s = iniFind(p->fp, "TCPDEST", p->section)))
        {
            p->tcp_destip = strdup(s);
        }

        if (findkwd(p, "TYPE", &p->type,
                    "rtu", TYPE_RTU,
                    "tcpserver", TYPE_TCP_SERVER,
                    "tcpclient", TYPE_TCP_CLIENT,
                    "custom",TYPE_CUSTOM,
                    NULL) == NAME_NOT_FOUND)
        {
            fprintf(stderr, "%s: missing required TYPE in section %s\n",
                    p->progname, p->section);
            return -1;
        }
    }
    else
    {
        fprintf(stderr, "%s:cant open inifile '%s'\n",
                p->progname, p->inifile);
        return -1;
    }
    return 0;
}

void usage(int argc, char **argv)
{
    printf("Usage:  %s [options]\n", argv[0]);
    printf("This is a userspace HAL program, typically loaded using the halcmd \"loadusr\" command:\n"
           "    loadusr vfs11_vfd [options]\n"
           "Options are:\n"
           "-I or --ini <inifile>\n"
           "    Use <inifile> (default: take ini filename from environment variable INI_FILE_NAME)\n"
           "-S or --section <section-name> (default 8)\n"
           "    Read parameters from <section_name> (default 'VFS11')\n"
           "-d or --debug\n"
           "    Turn on debugging messages. Toggled by USR1 signal.\n"
           "-m or --modbus-debug\n"
           "    Turn on modbus debugging.  This will cause all modbus messages\n"
           "    to be printed in hex on the terminal. Toggled by USR2 signal.\n"
           "-r or --report-device\n"
           "    Report device properties on console at startup\n");
}

static int recv_data(int fd, void *buf, int length_to_read, int flags )
{
    int rc;
    int s_rc;
    fd_set rset;
    struct timeval tv;
    struct timeval *p_tv;
    int msg_length = 0;

    FD_ZERO(&rset);
    FD_SET(fd, &rset);

    tv.tv_sec = 1;
    tv.tv_usec = 0;
    p_tv = &tv;
    
    while ((s_rc = select(fd+1, &rset, NULL, NULL, p_tv)) == -1)
    {
        if (errno == EINTR)
        {
            FD_ZERO(&rset);
            FD_SET(fd, &rset);
        }
        else
        {
            return -1;
        }
    }

    if (s_rc == 0)
    {
        return -1;
    }
        
    if(FD_ISSET(fd,&rset))
    {
        msg_length = recv(fd, buf, length_to_read, flags);
        if (msg_length == 0)
        {
            msg_length = -1;
        }

        if (msg_length == -1)
        {
            return -1;
        }     
    }

    return msg_length;

}

static int init_command(int fd, haldata_t *haldata, param_pointer p)
{
    char disenable_hardware[] = {'D','L','E','\r'};
    char disenable_external[] = {'D','E','C','\r'};
    char disenable_aiming[] = {'D','E','A','B','C','\r'};
    char Recv_buffer[20] = {0};

    int len = send(fd,disenable_hardware,sizeof(disenable_hardware),0);
    if(len < 0)
    {
        fprintf(stderr,"init_command send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"init_command receive error");
        return -1;
    }

    len = send(fd,disenable_external,sizeof(disenable_external),0);
    if(len < 0)
    {
        fprintf(stderr,"init_command send error");
        return -1;
    }
    size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"init_command receive error");
        return -1;
    }

    len = send(fd,disenable_aiming,sizeof(disenable_aiming),0);
    if(len < 0)
    {
        fprintf(stderr,"init_command send error");
        return -1;
    }
    size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"init_command receive error");
        return -1;
    }

    return 0;

}


//circuit_borad_temperature
static int get_device_temperature(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','C','T','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd,send_buffer,4,0);
    if(len < 0)
    {
        fprintf(stderr,"get_device_temperatur send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->device_temperature = (float)atof((const char *)++strchr_pointer);
    return 0;

}

//
static int get_circuit_borad_temperature(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','B','T','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd, send_buffer, 4, 0);
    if(len < 0)
    {
        fprintf(stderr,"circuit_borad_temperature send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"circuit_borad_temperature receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->circuit_borad_temperature = (float)atof((const char *)++strchr_pointer);
    return 0;

}

//
static int get_pulse_width(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','P','W','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd,send_buffer,4,0);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->pulse_width = (float)atof((const char *)++strchr_pointer);

    return 0;

}


//init 8  KHz
static int get_prr(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','P','R','R','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd,send_buffer,5,0);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20,0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->pulse_repetition_rate = (float)atof((const char *)++strchr_pointer);

    return 0;

}

//
static int get_device_status(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'S','T','A','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd, send_buffer, 4, 0);
    //usleep(175 * 4 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->status = (uint32_t)atoi((const char *)++strchr_pointer);

    return 0;


}

//get output power
static int get_output_power(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','O','P','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd, send_buffer, 4, 0);
    //usleep(175 * 5 + 700);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->output_power = (float)atof((const char *)++strchr_pointer);

    return 0;

}

static int get_current_monitor(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','I','M','O','N','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd, send_buffer, 6, 0);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }

    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->current_monitor = (float)atof((const char *)++strchr_pointer);
    return 0;

}

static int get_peak_power(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','P','P','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd, send_buffer, 4, 0);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }

    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->output_peak_power = (float)atof((const char *)++strchr_pointer);
    return 0;

}

static int get_min_current_setpoint(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','N','C','\r',0};
    char Recv_buffer[20] = {0};
    char *strchr_pointer = NULL;

    if(p->sockfd<0)
    {
        return -1;
    }

    int len = send(fd, send_buffer, 4, 0);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }

    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }
    strchr_pointer = strchr(Recv_buffer, ':' );
    if(strchr_pointer == NULL)
    {
        return -1;
    }
    Recv_buffer[size-1] = 0;
    *haldata->min_current_setpoint = (float)atof((const char *)++strchr_pointer);
    return 0;

}


static int set_work_model(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'D','P','M','\r',0};

    char Recv_buffer[20] = {0};
    char text[6] = {0};

    if(8 == (*haldata->apd_mode_index_cmd))
    {
        // continue
    }
    else
    {
        //pulse
        send_buffer[0] = 'E';

    }

    int len = send(fd, send_buffer, 4, 0);
    if(len < 0)
    {
        fprintf(stderr,"set_laser_model send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_model receive error");
        return -1;
    }

    return 0;

}

//
static int set_prr(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'S','P','R','R','\r'};
    char Recv_buffer[20] = {0};
    char text[20] = {0};
    int index = 0;

    int num = sprintf(text,"%.1f",*haldata->prr_cmd);
    if(-1 == num)
    {
        fprintf(stderr,"set_prr index error");
        return -1;
    }
    for(int i=4,j=0; j<num; i++,j++)
    {
        send_buffer[i] =  text[j];
        index = i;
    }
    send_buffer[index] = '\r';

    int len = send(fd, send_buffer, ++index, 0);
    //usleep(175 * index + 700);
    if(len < 0)
    {
        fprintf(stderr,"set_prr send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_prr receive error");
        return -1;
    }

    return 0;
}

static int set_pulse_width(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'S','P','W','0','0','0','0','0','\r',0};
    char Recv_buffer[20] = {0};
    char text[20] = {0};
    int index = 0;

    int num = sprintf(text,"%.1f",*haldata->pulse_width_cmd);
    if(-1 == num)
    {
        fprintf(stderr,"set_prr index error");
        return -1;
    }
    for(int i=3,j=0; j<num; i++,j++)
    {
        send_buffer[i] =  text[j];
        index = i;
    }
    send_buffer[index] = '\r';

    int len = send(fd, send_buffer, ++index, 0);
    if(len < 0)
    {
        fprintf(stderr,"set_pulse_duratin send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"get_device_temperatur receive error");
        return -1;
    }

    return 0;

}


//set operating power [%]
static int set_diode_current(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'S','D','C',0,0,0,0};
    char Recv_buffer[20] = {0};
    char text[10] = {0};
    int index = 0;

    int num = sprintf(text,"%.1f",*haldata->diode_current_cmd);
    if(-1 == num)
    {
        fprintf(stderr,"set_operating_power percent error");
        return -1;
    }
    for(int i=3,j=0; j<num; i++,j++)
    {
        send_buffer[i] =  text[j];
        index = i;
    }
    send_buffer[index] = '\r';

    int len = send(fd, send_buffer, ++index, 0);
    if(len < 0)
    {
        fprintf(stderr,"set_operating_power send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_operating_power receive error");
        return -1;
    }

    return 0;
}


static int set_guiding_light(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'A','B','F','\r',0};
    char Recv_buffer[20] = {0};


    if(*haldata->guide_cmd == true)
    {
        send_buffer[2] = 'N';
    }

    int len = send(fd, send_buffer, 4, 0);
    if(len < 0)
    {
        fprintf(stderr,"set_guiding_light send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_guiding_light receive error");
        return -1;
    }

    return 0;

}

static int set_laser_emission(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'E','M','O','N','\r','\r'};
    char Recv_buffer[20] = {0};
    int length = 5;

    if(*haldata->emission_cmd == false)
    {
        send_buffer[3] = 'F';
        send_buffer[4] = 'F';
        length = 6;
    }

    int len = send(fd, send_buffer, length, 0);
    if(len < 0)
    {
        fprintf(stderr,"set_laser_emission send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_emission receive error");
        return -1;
    }

    return 0;

}

static int set_laser_model(int fd, haldata_t *haldata, param_pointer p)
{
    char continue_buffer[20] = {'D','P','M','\r'};
    char pulse_buffer[20] = {'E','P','M','\r'};
    char Recv_buffer[20] = {0};
    int len = 0;


    if((*haldata->apd_mode_index_cmd) == 8)
    {
        len = send(fd, continue_buffer, sizeof(continue_buffer), 0);
    }
    else
    {
        len = send(fd, pulse_buffer, sizeof(pulse_buffer), 0);
    }
    if(len < 0)
    {
        fprintf(stderr,"set_laser_mddel send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_mddel receive error");
        return -1;
    }

    return 0;

}


static int reset_critical_error(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','C','E','1','1','2','3','4','5','6','1','2','3','\r'};
    char Recv_buffer[20] = {0};

    int len = send(fd, send_buffer, 14, 0);
    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }

    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_emission receive error");
        return -1;
    }
    return 0;
}

static int reset_error(int fd, haldata_t *haldata, param_pointer p)
{
    char send_buffer[20] = {'R','E','R','R','\r'};
    char Recv_buffer[20] = {0};
    int len = send(fd, send_buffer, 5, 0);

    if(len < 0)
    {
        fprintf(stderr,"get_read_test send error");
        return -1;
    }
    int size = recv_data(fd, Recv_buffer, 20, 0);
    if(size < 0)
    {
        fprintf(stderr,"set_laser_emission receive error");
        return -1;
    }
    return 0;
}

static int read_initial(int fd, haldata_t *haldata, param_pointer p)
{
    int retval = init_command(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_device_temperature(fd,haldata,p);
    if(retval < 0)
        return 1;
    retval = get_circuit_borad_temperature(fd,haldata,p);
    if(retval < 0)
        return 1;

    return 0;

}

static int read_data(int fd, haldata_t *haldata, param_pointer p)
{
    int retval;
    static int Cnt = 0;

    if (!p->read_initial_done)
    {
        if ((retval = read_initial(fd, haldata, p)))
            return retval;
        else
            p->read_initial_done = 1;
    }

    // we always at least read the main status register SR_INV_OPSTATUS
    switch(Cnt)
    {
        case 0:
            retval = get_prr(fd,haldata,p);
            Cnt++;
            break;
        case 1:
            retval = get_device_temperature(fd,haldata,p);
            Cnt++;
            break;
        case 2:
            retval = get_circuit_borad_temperature(fd,haldata,p);
            Cnt++;
            break;
        case 3:
            retval = get_device_status(fd,haldata,p);
            Cnt++;
            break;
        case 4:
            retval = get_pulse_width(fd,haldata,p);
            Cnt++;
            break;
        case 5:
            retval = get_output_power(fd,haldata,p);
            Cnt++;
            break;
        case 6:
            retval = get_current_monitor(fd,haldata,p);
            Cnt++;
            break;
        case 7:
            retval = get_peak_power(fd,haldata,p);
            Cnt++;
            break;
        case 8:
            retval = get_min_current_setpoint(fd,haldata,p);
            Cnt = 0;
            break;
    }
    if(retval  < 0)
        return 1;
    return 0;
}

static int write_data(int fd, haldata_t *haldata, param_pointer p)
{
    //waiting for Laser ready
    if (!*(haldata->enabled))
    {
        return 0;
    }

    if(*haldata->apd_mode_index_cmd != p->old_apd_mode_index)
    {
        if(set_laser_model(fd,haldata,p) < 0)
            return 1;
        p->old_apd_mode_index = *haldata->apd_mode_index_cmd;
    }
    if(*haldata->prr_cmd != p->old_prr)
    {
        if(set_prr(fd,haldata,p) < 0)
            return 1;
        p->old_prr = *haldata->prr_cmd;
        get_prr(fd,haldata,p);
    }
    if(*haldata->pulse_width_cmd != p->old_pulse_width_cmd)
    {
        if(set_pulse_width(fd,haldata,p) < 0)
            return 1;
        p->old_pulse_width_cmd = *haldata->pulse_width_cmd;
        get_prr(fd,haldata,p);
    }

    if(*haldata->diode_current_cmd != p->old_diode_current_cmd)
    {
        if(set_diode_current(fd,haldata,p) < 0)
            return 1;
        p->old_diode_current_cmd = *haldata->diode_current_cmd;
        get_output_power(fd,haldata,p);
        get_current_monitor(fd,haldata,p);
    }

    if(*haldata->guide_cmd != p->old_guide)
    {
        if(set_guiding_light(fd,haldata,p) < 0)
            return 1;
        p->old_guide = *haldata->guide_cmd;
    }

    if(*haldata->emission_cmd != p->old_emission)
    {
        if(set_laser_emission(fd,haldata,p) < 0)
            return 1;
        p->old_emission = *haldata->emission_cmd;
    }

    return 0;
}


#define PIN(x)                  \
    do {                        \
    status = (x);                   \
    if ((status) != 0)              \
        return status;              \
    } while (0)

int hal_setup(int id, haldata_t *h, const char *name)
{
    int status;
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->status), id, "%s.device-status", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->circuit_borad_temperature), id, "%s.circuit-borad-temperature", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->device_temperature), id, "%s.device-temperature", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->output_power), id, "%s.output-power", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->current_setpoint), id, "%s.current-setpoint", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->min_current_setpoint), id, "%s.min-current-setpoint", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->current_monitor), id, "%s.current-monitor", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->output_peak_power), id, "%s.output-peak-power", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->pulse_repetition_rate), id, "%s.pulse-repetition-rate", name));
    PIN(hal_pin_float_newf(HAL_OUT, &(h->pulse_width), id, "%s.pulse-width", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->error_code), id, "%s.error-code", name));
    PIN(hal_pin_u32_newf(HAL_OUT, &(h->apd_mode_index), id, "%s.apd-mode-index", name));
    PIN(hal_pin_bit_newf(HAL_OUT, &(h->modbus_ok), id, "%s.modbus-ok", name));

    PIN(hal_pin_float_newf(HAL_IN, &(h->prr_cmd), id, "%s.prr-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->pulse_width_cmd), id, "%s.pulse-width-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->diode_current_cmd), id, "%s.diode-current-cmd", name));
    PIN(hal_pin_float_newf(HAL_IN, &(h->apd_mode_index_cmd), id, "%s.apd-mode-index-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->emission_cmd), id, "%s.emission-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->guide_cmd), id, "%s.guide-cmd", name));
    PIN(hal_pin_bit_newf(HAL_IN, &(h->enabled), id, "%s.enabled", name));

    return 0;
}
#undef PIN

int set_defaults(param_pointer p)
{
    haldata_t *h = p->haldata;

    *(h->status) = 0;
    *(h->device_temperature) = 0.0;
    *(h->modbus_ok) = false;

    *(h->guide_cmd) = false;
    *(h->emission_cmd) = false;
    *(h->apd_mode_index_cmd) = 0;
    *(h->prr_cmd) = 0.0;
    *(h->diode_current_cmd) = 0.0;
    *(h->pulse_width_cmd) = 0.0;

    h->looptime = 0.05;
    h->speed_tolerance = 0.01;      // output frequency within 1% of target frequency

    return 0;
}

int main(int argc, char **argv)
{
    struct timespec loop_timespec, remaining;
    struct sockaddr_in servaddr;
    int opt;
    int rc;
    param_pointer p = &param;
    int retval = 0;
    retval = -1;
    p->progname = argv[0];
    connection_state = NOT_CONNECTED;
    p->inifile = getenv("INI_FILE_NAME");
    char devnull[TCP_MAX_DATA_LENGTH];

    while ((opt = getopt_long(argc, argv, option_string, long_options, NULL)) != -1)
    {
        switch(opt)
        {
            case 'n':
                p->modname = strdup(optarg);
                break;
            case 'm':
//                p->modbus_debug = 1;
                break;
            case 'd':
//                p->debug = 1;
                break;
            case 'S':
                p->section = optarg;
                break;
            case 'I':
                p->inifile = optarg;
                break;
            case 'r':
                p->report_device = 1;
                break;
            case 'h':
            default:
//      usage(argc, argv);
                exit(0);
        }
    }

    if (p->inifile)
    {
        if (read_ini(p))
            goto finish;
        if (!p->modname)
            p->modname = "ipglaser";
    }
    else
    {
        fprintf(stderr, "%s: ERROR: no inifile - either use '--ini inifile' or set INI_FILE_NAME environment variable\n", p->progname);
        goto finish;
    }

    signal(SIGINT, quit);
    signal(SIGTERM, quit);
    signal(SIGUSR1, toggle_debug);
    signal(SIGUSR2, toggle_modbus_debug);

    // create HAL component
    p->hal_comp_id = hal_init(p->modname);
    if ((p->hal_comp_id < 0) || (connection_state == DONE))
    {
        fprintf(stderr, "%s: ERROR: hal_init(%s) failed: HAL error code=%d\n",
                p->progname, p->modname, p->hal_comp_id);
        retval = p->hal_comp_id;
        goto finish;
    }

    // grab some shmem to store the HAL data in
    p->haldata = (haldata_t *)hal_malloc(sizeof(haldata_t));
    if ((p->haldata == 0) || (connection_state == DONE))
    {
        fprintf(stderr, "%s: ERROR: unable to allocate shared memory\n", p->modname);
        retval = -1;
        goto finish;
    }
    if (hal_setup(p->hal_comp_id,p->haldata, p->modname))
        goto finish;

    set_defaults(p);
    hal_ready(p->hal_comp_id);

    DBG("using libmodbus version %s\n", LIBMODBUS_VERSION_STRING);

    switch (p->type)
    {

        case  TYPE_TCP_CLIENT:
            connection_state = OPENING;
            if( (p->sockfd = socket(AF_INET,SOCK_STREAM,0)) == -1)
            {
                printf(" create socket error: %s (errno :%d)\n",strerror(errno),errno);
                return 0;
            }
            memset(&servaddr,0,sizeof(servaddr));
            servaddr.sin_family = AF_INET;
            servaddr.sin_port = htons(p->tcp_portno);

            if( inet_pton(AF_INET,p->tcp_destip, &servaddr.sin_addr) <=0 )
            {
                printf("inet_pton error for %s\n",argv[1]);
                return 0;
            }

            if( connect(p->sockfd,(struct sockaddr*)&servaddr,sizeof(servaddr)) <0)
            {
                printf(" connect socket error: %s(errno :%d)\n",strerror(errno),errno);
                return 0;
            }

            DBG("main: TCP connected to %s:%d\n", p->tcp_destip, p->tcp_portno);
            break;

        case TYPE_CUSTOM:
//            connection_state = OPENING;
//            p->fd = open(p->device,O_RDWR|O_NOCTTY|O_EXCL); // |O_NDELAY
//            if(p->fd < 0)
//            {
//                fprintf(stderr, "%s:%s ERROR",p->device,p->progname);
//            }
//            tcgetattr(p->fd, &old_tios);
//            int ret = set_port_attr(p->fd,p->baud,p->bits,p->stopbits,p->parity,1,255);
//            if(ret < 0)
//            {
//                fprintf(stderr, "%s:%s ERROR param Setting",p->device,p->progname);
//            }
//            sleep(p->reconnect_delay);
//            tcflush(p->fd, TCIOFLUSH);//clear input and output buffer

            break;

        default:
            fprintf(stderr, "%s: ERROR: invalid connection type %d\n",
                    p->progname, p->type);
            goto finish;
    }
    sleep(3);
    connection_state = CONNECTED;
    while (connection_state != DONE)
    {

        while (connection_state == CONNECTED)
        {
            if ((retval = read_data(p->sockfd, p->haldata, p)))
            {
                p->modbus_ok = 0;
            }
            else
            {
                p->modbus_ok++;
            }
            if (p->modbus_ok > MODBUS_MIN_OK)
            {
                *(p->haldata->modbus_ok) = 1;
            }
            else
            {
                *(p->haldata->modbus_ok) = 0;
            }
            if ((retval = write_data(p->sockfd, p->haldata, p)))
            {
                p->modbus_ok = 0;
                if (retval == 1)
                {
                    connection_state = RECOVER;
                }
            }
            else
            {
                p->modbus_ok++;
            }
            if (p->modbus_ok > MODBUS_MIN_OK)
            {
                *(p->haldata->modbus_ok) = 1;
            }
            else
            {
                *(p->haldata->modbus_ok) = 0;
            }
            /* don't want to scan too fast, and shouldn't delay more than a few seconds */
            if (p->haldata->looptime < 0.001) p->haldata->looptime = 0.001;
            if (p->haldata->looptime > 1) p->haldata->looptime = 1;
            loop_timespec.tv_sec = (time_t)(p->haldata->looptime);
            loop_timespec.tv_nsec = (long)((p->haldata->looptime - loop_timespec.tv_sec) * 1000000000l);
            nanosleep(&loop_timespec, &remaining);
        }

        switch (connection_state)
        {
            case DONE:
                // cleanup actions before exiting.
                tcflush(p->fd, TCIOFLUSH);//clear input and output buffer
                DBG("cleanup actions before exiting");
                break;

            case RECOVER:
                DBG("recover\n");
                set_defaults(p);
                p->read_initial_done = 0;
                // reestablish connection to slave
                switch (p->type)
                {

                    case TYPE_RTU:
                    case TYPE_TCP_CLIENT:
                        rc = recv(p->sockfd, devnull, TCP_MAX_DATA_LENGTH, MSG_DONTWAIT);
                        shutdown(p->sockfd, SHUT_RDWR);
                        close(p->sockfd);
                        p->sockfd = -1;
                        while ((connection_state != CONNECTED) &&
                               (connection_state != DONE))
                        {
                            sleep(p->reconnect_delay);
                            if (connect(p->sockfd,(struct sockaddr*)&servaddr,sizeof(servaddr)))
                            {
                                connection_state = CONNECTED;
                                DBG("rtu/tcpclient reconnect\n");
                            }
                            else
                            {
                                fprintf(stderr, "%s: recovery: modbus_connect(): %s\n",
                                        p->progname, modbus_strerror(errno));
                            }
                        }

                        DBG("rtu/tcp reconnect\n");
                        break;

                    case TYPE_CUSTOM:
                        ioctl(p->fd, TCFLSH, 2);  //clear input and output buffer
                        close(p->fd);
                        p->fd = -1;
                        while ((connection_state != CONNECTED) &&
                               (connection_state != DONE))
                        {
                            sleep(p->reconnect_delay);
//                            p->fd = open(p->device,O_RDWR|O_NDELAY|O_NOCTTY|O_EXCL);
//                            if(p->fd < 0)
//                            {
//                                fprintf(stderr, "%s:%s ERROR",p->device,p->progname);
//                            }
//                            else
//                            {
//                                connection_state = CONNECTED;
//                                DBG("custom reconnect\n");
//                            }
//                            tcgetattr(p->fd, &old_tios);
//                            int ret = set_port_attr(p->fd,p->baud,p->bits,p->stopbits,p->parity,1,100);
//                            if(ret < 0)
//                            {
//                                fprintf(stderr, "%s:%s ERROR param Setting",p->device,p->progname);
//                            }
                        }
                        break;

                    case TYPE_TCP_SERVER:
                        DBG("tcpserver reconnect\n");
                        break;

                    default:
                        break;
                }
                break;
            default:
                ;
                break;
        }
    }
    retval = 0;

finish:
    windup(p);
    return retval;
}


